{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# CombinedImuFactor\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/CombinedImuFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `CombinedImuFactor` is an advanced IMU factor in GTSAM that offers several improvements over the standard `ImuFactor`:\n",
    "1.  **Bias Evolution:** It explicitly models the evolution of IMU biases between time steps $i$ and $j$ using a random walk model.\n",
    "2.  **6-Way Factor:** Consequently, it connects *six* variables: Pose_i, Vel_i, Bias_i, Pose_j, Vel_j, and Bias_j.\n",
    "3.  **Combined Preintegration:** It uses `PreintegratedCombinedMeasurements` which propagates a full 15x15 covariance matrix, accounting for correlations between the preintegrated state and the biases, as well as the bias random walk noise.\n",
    "\n",
    "This factor is generally preferred when bias stability is a concern or when modeling the time-varying nature of biases is important for accuracy. It eliminates the need for separate `BetweenFactor`s on bias variables."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Formulation\n",
    "\n",
    "The `CombinedImuFactor` has a 15-dimensional error vector, conceptually split into two parts:\n",
    "\n",
    "1.  **Preintegration Error (9 dimensions):** This part is identical in concept to the error in `ImuFactor`. It measures the discrepancy between the relative motion predicted by the `PreintegratedCombinedMeasurements` (using the current estimate of $b_i$) and the relative motion implied by the states $X_i = (R_i, p_i, v_i)$ and $X_j = (R_j, p_j, v_j)$.\n",
    "    $$ e_{imu} = \\text{NavState::Logmap}( \\text{pim.predict}(X_i, b_i)^{-1} \\otimes X_j ) $$ \n",
    "\n",
    "2.  **Bias Random Walk Error (6 dimensions):** This part measures how much the change in bias ($b_j - b_i$) deviates from the expected zero-mean random walk. \n",
    "    $$ e_{bias} = b_j - b_i $$ \n",
    "\n",
    "The total error vector is $e = [e_{imu}; e_{bias}]$.\n",
    "\n",
    "The factor's noise model (derived from `pim.preintMeasCov()`) is a 15x15 matrix that correctly weights these two error components, accounting for the propagated uncertainty from IMU measurements, initial bias uncertainty, and the bias random walk process noise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Constructor**: `CombinedImuFactor(keyPose_i, keyVel_i, keyPose_j, keyVel_j, keyBias_i, keyBias_j, pim)`: Creates the factor, linking the six state/bias keys and providing the combined preintegrated measurements.\n",
    "- **`preintegratedMeasurements()`**: Returns a const reference to the stored `PreintegratedCombinedMeasurements` object.\n",
    "- **`evaluateError(pose_i, vel_i, pose_j, vel_j, bias_i, bias_j)`**: Calculates the 15-dimensional error vector given the current values of the connected variables. Also computes Jacobians if requested.\n",
    "- **`noiseModel()`**: Returns the 15x15 noise model associated with the preintegrated measurement and bias evolution uncertainty.\n",
    "- **`print` / `equals`**: Standard factor methods."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "Create combined parameters, create a combined PIM object, define keys (including two bias keys), and construct the factor."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Created CombinedImuFactor:\n",
      "CombinedImuFactor(x0,v0,x1,v1,b0,b1)\n",
      "  preintegrated measurements:\n",
      "    deltaTij = 0.1\n",
      "    deltaRij.ypr = ( 0 -0  0)\n",
      "    deltaPij =        0        0 -0.04905\n",
      "    deltaVij =      0      0 -0.981\n",
      "    gyrobias = 0 0 0\n",
      "    acc_bias = 0 0 0\n",
      "\n",
      "  preintMeasCov [      0.00011            0            0            0  1.53772e-06            0            0  4.85595e-05            0            0            0            0      4.5e-11            0            0\n",
      "           0      0.00011            0 -1.53772e-06            0            0 -4.85595e-05            0            0            0            0            0            0      4.5e-11            0\n",
      "           0            0      0.00011            0            0            0            0            0            0            0            0            0            0            0      4.5e-11\n",
      "           0 -1.53772e-06            0  3.69908e-06            0            0  5.60718e-05            0            0    1.425e-10            0            0            0  -2.6487e-13            0\n",
      " 1.53772e-06            0            0            0  3.69908e-06            0            0  5.60718e-05            0            0    1.425e-10            0   2.6487e-13            0            0\n",
      "           0            0            0            0            0   3.6585e-06            0            0      5.5e-05            0            0    1.425e-10            0            0            0\n",
      "           0 -4.85595e-05            0  5.60718e-05            0            0   0.00113017            0            0      4.5e-09            0            0            0  -1.1772e-11            0\n",
      " 4.85595e-05            0            0            0  5.60718e-05            0            0   0.00113017            0            0      4.5e-09            0   1.1772e-11            0            0\n",
      "           0            0            0            0            0      5.5e-05            0            0       0.0011            0            0      4.5e-09            0            0            0\n",
      "           0            0            0    1.425e-10            0            0      4.5e-09            0            0        1e-07            0            0            0            0            0\n",
      "           0            0            0            0    1.425e-10            0            0      4.5e-09            0            0        1e-07            0            0            0            0\n",
      "           0            0            0            0            0    1.425e-10            0            0      4.5e-09            0            0        1e-07            0            0            0\n",
      "     4.5e-11            0            0            0   2.6487e-13            0            0   1.1772e-11            0            0            0            0        1e-09            0            0\n",
      "           0      4.5e-11            0  -2.6487e-13            0            0  -1.1772e-11            0            0            0            0            0            0        1e-09            0\n",
      "           0            0      4.5e-11            0            0            0            0            0            0            0            0            0            0            0        1e-09 ]\n",
      "  noise model: Gaussian [\n",
      "\t96.6351, 0, 0, 0, 91.8245, 0, -0, -8.70782, -0, 0, 0.261002, 0, -4.27039, 0, 0;\n",
      "\t0, 96.6351, 0, -91.8245, 0, 0, 8.70782, -0, -0, -0.261002, 0, 0, 0, -4.27039, 0;\n",
      "\t0, 0, 95.3463, 0, 0, 0, -0, -0, -0, 0, 0, 0, 0, 0, -4.29058;\n",
      "\t0, 0, 0, 1044.19, 0, 0, -51.806, -0, -0, 0.843301, 0, 0, 0, -0.333286, 0;\n",
      "\t0, 0, 0, 0, 1044.19, 0, -0, -51.806, -0, 0, 0.843301, 0, 0.333286, 0, 0;\n",
      "\t0, 0, 0, 0, 0, 1049.15, -0, -0, -52.4575, 0, 0, 0.865549, 0, 0, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 29.746, -0, -0, -1.33857, 0, 0, 0, 0.35017, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 29.746, -0, 0, -1.33857, 0, -0.35017, 0, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 0, 30.1511, 0, 0, -1.3568, 0, 0, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, 0, 0, 5.26625e-20, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, -5.26625e-20, 0, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3162.28, 0, 0, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8, 0, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8, 0;\n",
      "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31622.8\n",
      "]\n",
      "\n",
      "Error vector (should be near zero): [0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]\n",
      "Factor error (0.5 * ||error||^2_Sigma): 0.0\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "from gtsam import (PreintegrationCombinedParams,\n",
    "                   PreintegratedCombinedMeasurements, CombinedImuFactor,\n",
    "                   NonlinearFactorGraph, Values, Pose3, NavState)\n",
    "from gtsam.imuBias import ConstantBias\n",
    "from gtsam.symbol_shorthand import X, V, B\n",
    "\n",
    "# 1. Create Combined Parameters and PIM (as in PreintegratedCombinedMeasurements example)\n",
    "params = PreintegrationCombinedParams.MakeSharedU(9.81)\n",
    "accel_noise_sigma = 0.1\n",
    "gyro_noise_sigma = 0.01\n",
    "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n",
    "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n",
    "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n",
    "bias_acc_rw_sigma = 0.001\n",
    "bias_gyro_rw_sigma = 0.0001\n",
    "params.setBiasAccCovariance(np.eye(3) * bias_acc_rw_sigma**2)\n",
    "params.setBiasOmegaCovariance(np.eye(3) * bias_gyro_rw_sigma**2)\n",
    "initial_bias_cov = np.diag(np.full(6, 1e-3)) # Example initial bias uncertainty\n",
    "params.setBiasAccOmegaInit(initial_bias_cov)\n",
    "bias_hat = ConstantBias()\n",
    "pim = PreintegratedCombinedMeasurements(params, bias_hat)\n",
    "\n",
    "# Integrate some dummy measurements\n",
    "dt = 0.01\n",
    "acc_meas = np.array([0.0, 0.0, -9.81]) # Stationary\n",
    "gyro_meas = np.array([0.0, 0.0, 0.0]) # Stationary\n",
    "for _ in range(10):\n",
    "    pim.integrateMeasurement(acc_meas, gyro_meas, dt)\n",
    "\n",
    "# 2. Define Symbolic Keys using shorthand\n",
    "# Keys: X(0), V(0), B(0) for time i\n",
    "#       X(1), V(1), B(1) for time j\n",
    "\n",
    "# 3. Create the CombinedImuFactor\n",
    "# The 15x15 noise model is automatically derived from pim.preintMeasCov()\n",
    "combined_imu_factor = CombinedImuFactor(\n",
    "    X(0), V(0),\n",
    "    X(1), V(1),\n",
    "    B(0), B(1),\n",
    "    pim)\n",
    "\n",
    "print(\"Created CombinedImuFactor:\")\n",
    "combined_imu_factor.print()\n",
    "\n",
    "# 4. Example: Evaluate error with perfect states & no bias change (should be near zero)\n",
    "graph = NonlinearFactorGraph()\n",
    "graph.add(combined_imu_factor)\n",
    "\n",
    "values = Values()\n",
    "pose_i = Pose3()\n",
    "vel_i = np.zeros(3)\n",
    "bias_i = ConstantBias() # Matches bias_hat used in PIM\n",
    "bias_j = bias_i # Assume no bias change for zero error check\n",
    "\n",
    "# Predict state j using the PIM\n",
    "nav_state_i = NavState(pose_i, vel_i)\n",
    "nav_state_j = pim.predict(nav_state_i, bias_i) # Use bias_i=bias_hat\n",
    "pose_j = nav_state_j.pose()\n",
    "vel_j = nav_state_j.velocity()\n",
    "\n",
    "values.insert(X(0), pose_i)\n",
    "values.insert(V(0), vel_i)\n",
    "values.insert(B(0), bias_i)\n",
    "values.insert(X(1), pose_j)\n",
    "values.insert(V(1), vel_j)\n",
    "values.insert(B(1), bias_j)\n",
    "\n",
    "error_vector = combined_imu_factor.evaluateError(\n",
    "    pose_i, vel_i, pose_j, vel_j, bias_i, bias_j)\n",
    "print(\"\\nError vector (should be near zero):\", error_vector)\n",
    "print(\"Factor error (0.5 * ||error||^2_Sigma):\", graph.error(values))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [CombinedImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.h)\n",
    "- [CombinedImuFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.cpp)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
